#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/Image.h>
#include<cv_bridge/cv_bridge.h>
#include<opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
//#include <pcl/conversions.h>
#include <map>
#include <chrono>
#include "cctag/Detection.hpp"

using namespace std;
cv::Mat img_;

void drawMarkers(const boost::ptr_list<cctag::ICCTag>& markers, cv::Mat& image)
{
    // drawing settings
    const int radius{10};
    const int fontSize{3};
    const int thickness{3};
    const int fontFace{cv::FONT_HERSHEY_SIMPLEX};

    for(const auto& marker : markers)
    {
        // center of the marker
        const cv::Point center = cv::Point(marker.x(), marker.y());
        const auto rescaledOuterEllipse = marker.rescaledOuterEllipse();

        // check the status and draw accordingly, green for valid, red otherwise
        if(marker.getStatus() == cctag::status::id_reliable)
        {
            const cv::Scalar color = cv::Scalar(0, 255, 0, 255);
            // draw the center
            cv::circle(image, center, radius, color, thickness);
            // write the marker ID
            cv::putText(image, std::to_string(marker.id()), center, fontFace, fontSize, color, thickness);
            // draw external ellipse
            cv::ellipse(image,
                        center,
                        cv::Size(rescaledOuterEllipse.a(), rescaledOuterEllipse.b()),
                        rescaledOuterEllipse.angle() * 180 / boost::math::constants::pi<double>(),
                        0,
                        360,
                        color,
                        thickness);
        }
        else
        {
            // the same for invalid markers but in red
            const cv::Scalar color = cv::Scalar(0, 0, 255, 255);
            cv::circle(image, center, radius, color, thickness);
            cv::putText(image, std::to_string(marker.id()), center, fontFace, fontSize, color, thickness);
        }
    }
}

void img_CB(const sensor_msgs::ImageConstPtr& image_msg){
    cv_bridge::CvImagePtr image_front;
    image_front = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
    img_ = (*image_front).image;
    cv::cvtColor(img_, img_, CV_BGR2GRAY);

    // set up the parameters
    const std::size_t nCrowns{3};
    cctag::Parameters params(nCrowns);
    // choose a cuda pipe
    const int pipeId{0};
    // an arbitrary id for the frame
    const int frameId{0};
    // process the image
    boost::ptr_list<cctag::ICCTag> markers{};
    cctagDetection(markers, pipeId, frameId, img_, params);

    drawMarkers(markers,img_);
    cv::imshow("img",img_);
    cv::waitKey(1);
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "det_handle_img");
    ros::NodeHandle nh;
    
    ros::Subscriber image_sub=nh.subscribe("/camera/image_raw", 5,img_CB);


    ros::spin();

    return 0;
}
